/*
 * Copyright (c) 2011, okazoh_tk. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *  - Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *  - Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *  - Neither the name of the copyright holder nor the names of its contributors
 *    may be used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include <math.h>
#include "alkes/math/Math.h"
#include "alkes/math/Matrix.h"

namespace alkes {

#define SELF(x, y)  M(this, x, y)
#define M(m, x, y)  ((m)->m_[x][y])

void Matrix::identity()
{
    memset(m_, 0, sizeof(m_));
    AL_M(0, 0) = 1.0f;
    AL_M(1, 1) = 1.0f;
    AL_M(2, 2) = 1.0f;
    AL_M(3, 3) = 1.0f;
}

void Matrix::multiply(const Matrix& lh, const Matrix& rh)
{
    Matrix tmp;
    int i;

    for (i = 0; i < 4; ++i)
    {
        M(&tmp, i, 0) =
            M(&lh, i, 0) * M(&rh, 0, 0) +
            M(&lh, i, 1) * M(&rh, 1, 0) +
            M(&lh, i, 2) * M(&rh, 2, 0) +
            M(&lh, i, 3) * M(&rh, 3, 0);

        M(&tmp, i, 1) =
            M(&lh, i, 0) * M(&rh, 0, 1) +
            M(&lh, i, 1) * M(&rh, 1, 1) +
            M(&lh, i, 2) * M(&rh, 2, 1) +
            M(&lh, i, 3) * M(&rh, 3, 1);

        M(&tmp, i, 2) =
            M(&lh, i, 0) * M(&rh, 0, 2) +
            M(&lh, i, 1) * M(&rh, 1, 2) +
            M(&lh, i, 2) * M(&rh, 2, 2) +
            M(&lh, i, 3) * M(&rh, 3, 2);

        M(&tmp, i, 3) =
            M(&lh, i, 0) * M(&rh, 0, 3) +
            M(&lh, i, 1) * M(&rh, 1, 3) +
            M(&lh, i, 2) * M(&rh, 2, 3) +
            M(&lh, i, 3) * M(&rh, 3, 3);
    }

    memcpy(m_, tmp.m_, sizeof(m_));
}

void Matrix::perspective(float fovy, float aspect, float znear, float zfar)
{
    float h = tanf(fovy / 360.0f * PI) * znear;
    float w = h * aspect;

    frustum(-w, w, -h, h, znear, zfar);
}

void Matrix::frustum(float left, float right, float bottom, float top, float znear, float zfar)
{
    Matrix tmp;

    if (!frustum(tmp, left, right, bottom, top, znear, zfar))
        return;

    multiply(tmp, *this);
}

void Matrix::ortho(float left, float right, float bottom, float top, float znear, float zfar)
{
    Matrix tmp;

    if (!ortho(tmp, left, right, bottom, top, znear, zfar))
        return ;

    multiply(tmp, *this);
}

void Matrix::lookAt(const Vector3D& eye, const Vector3D& at, const Vector3D& up)
{
    Matrix tmp;

    lookAt(tmp, eye, at, up);

    multiply(tmp, *this);
}

void Matrix::translate(float x, float y, float z)
{
    SELF(3, 0) += (SELF(0, 0) * x + SELF(1, 0) * y + SELF(2, 0) * z);
    SELF(3, 1) += (SELF(0, 1) * x + SELF(1, 1) * y + SELF(2, 1) * z);
    SELF(3, 2) += (SELF(0, 2) * x + SELF(1, 2) * y + SELF(2, 2) * z);
    SELF(3, 3) += (SELF(0, 3) * x + SELF(1, 3) * y + SELF(2, 3) * z);
}

void Matrix::scale(float xscale, float yscale, float zscale)
{
    SELF(0, 0) *= xscale;
    SELF(0, 1) *= xscale;
    SELF(0, 2) *= xscale;
    SELF(0, 3) *= xscale;
    SELF(1, 0) *= yscale;
    SELF(1, 1) *= yscale;
    SELF(1, 2) *= yscale;
    SELF(1, 3) *= yscale;
    SELF(2, 0) *= zscale;
    SELF(2, 1) *= zscale;
    SELF(2, 2) *= zscale;
    SELF(2, 3) *= zscale;
}

void Matrix::rotate(float angle, float x, float y, float z)
{
    float len = sqrtf(x * x + y * y + z * z);

    if (len > 0.0f)
    {
        Matrix tmp;
        float rad = angle * PI / 180.0f;
        float s = sinf(rad);
        float c = cosf(rad);
        float c_1 = 1.0f - c;

        M(&tmp, 0, 0) = (x * x * c_1) + c;
        M(&tmp, 0, 1) = (c_1 * x * y) - z * s;
        M(&tmp, 0, 2) = (c_1 * z * x) + y * s;
        M(&tmp, 0, 3) = 0.0f;

        M(&tmp, 1, 0) = (c_1 * x * y) + z * s;
        M(&tmp, 1, 1) = (c_1 * y * y) + c;
        M(&tmp, 1, 2) = (c_1 * y * z) - x * s;
        M(&tmp, 1, 3) = 0.0f;

        M(&tmp, 2, 0) = (c_1 * z * x) - y * s;
        M(&tmp, 2, 1) = (c_1 * y * z) + x * s;
        M(&tmp, 2, 2) = (c_1 * z * z) + c;
        M(&tmp, 2, 3) = 0.0f; 

        M(&tmp, 3, 0) = 0.0f;
        M(&tmp, 3, 1) = 0.0f;
        M(&tmp, 3, 2) = 0.0f;
        M(&tmp, 3, 3) = 1.0f;

        multiply(tmp, *this);
    }
}

void Matrix::perspective(Matrix& m, float fovy, float aspect, float znear, float zfar)
{
    float h = tanf(fovy / 360.0f * PI) * znear;
    float w = h * aspect;

    frustum(m, -w, w, -h, h, znear, zfar);
}

bool Matrix::frustum(Matrix& m, float left, float right, float bottom, float top, float znear, float zfar)
{
    float dx = right - left;
    float dy = top - bottom;
    float dz = zfar - znear;

    if ((znear <= 0.0f) || (zfar <= 0.0f) || (dx <= 0.0f) || (dy <= 0.0f) || (dz <= 0.0f))
        return false;

    M(&m, 0, 0) = 2.0f * znear / dx;
    M(&m, 0, 1) = M(&m, 0, 2) = M(&m, 0, 3) = 0.0f;

    M(&m, 1, 1) = 2.0f * znear / dy;
    M(&m, 1, 0) = M(&m, 1, 2) = M(&m, 1, 3) = 0.0f;

    M(&m, 2, 0) = (right + left) / dx;
    M(&m, 2, 1) = (top + bottom) / dy;
    M(&m, 2, 2) = -(znear + zfar) / dz;
    M(&m, 2, 3) = -1.0f;

    M(&m, 3, 2) = -2.0f * znear * zfar / dz;
    M(&m, 3, 0) = M(&m, 3, 1) = M(&m, 3, 3) = 0.0f;

    return true;
}

bool Matrix::ortho(Matrix& m, float left, float right, float bottom, float top, float znear, float zfar)
{
    float dx = right - left;
    float dy = top - bottom;
    float dz = zfar - znear;

    if ((dx == 0.0f) || (dy == 0.0f) || (dz == 0.0f))
        return false;

    m.identity();

    M(&m, 0, 0) = 2.0f / dx;
    M(&m, 3, 0) = -(right + left) / dx;
    M(&m, 1, 1) = 2.0f / dy;
    M(&m, 3, 1) = -(top + bottom) / dy;
    M(&m, 2, 2) = -2.0f / dz;
    M(&m, 3, 2) = -(znear + zfar) / dz;

    return true;
}

void Matrix::lookAt(Matrix& m, const Vector3D& eye, const Vector3D& at, const Vector3D& up)
{
    Vector3D zaxis = eye - at;
    Vector3D xaxis = up.cross(zaxis);
    Vector3D yaxis = zaxis.cross(xaxis);

    zaxis.normalize();
    xaxis.normalize();

    m.identity();

    M(&m, 0, 0) = xaxis.x;
    M(&m, 0, 1) = xaxis.y;
    M(&m, 0, 2) = xaxis.z;

    M(&m, 1, 0) = yaxis.x;
    M(&m, 1, 1) = yaxis.y;
    M(&m, 1, 2) = yaxis.z;

    M(&m, 2, 0) = -zaxis.x;
    M(&m, 2, 1) = -zaxis.y;
    M(&m, 2, 2) = -zaxis.z;

    M(&m, 3, 0) = -eye.x;
    M(&m, 3, 1) = -eye.y;
    M(&m, 3, 2) = -eye.z;
}

}
